#include "pch.h"
#include "UndirectedGraph.h"
#include <boost/dynamic_bitset.hpp>

struct Node
{
	int Index = -1;
	int Distance = INT_MAX;

	Node(int vIndex, int vDistance) : Index(vIndex), Distance(vDistance) {}

	bool operator < (const Node& rhs) const
	{
		//Note: The shorter the distance, the higher the priority
		return Distance > rhs.Distance;
	}

	bool operator <= (const Node& rhs) const
	{
		return Distance >= rhs.Distance;
	}
};

CUndirectedGraph::CUndirectedGraph(int vNodeCount) : m_NodeCount(vNodeCount)
{
	_ASSERTE(m_NodeCount > 0);

	m_AdjacentMatrix = Eigen::SparseMatrix<int>(m_NodeCount, m_NodeCount);

	m_DenseAdjacentMatrix = Eigen::MatrixXi(m_NodeCount, m_NodeCount);

#pragma omp parallel for
	for (int i = 0; i < m_NodeCount; i++)
	{
		for (int k = 0; k < m_NodeCount; k++)
		{
			m_DenseAdjacentMatrix(i, k) = (i == k ? 0 : m_MaxDistance);
		}
	}
}

//****************************************************************************************************
//FUNCTION:
bool CUndirectedGraph::addEdge(int vStartNode, int vEndNode)
{
	if (vStartNode < 0 || vStartNode >= m_NodeCount) return false;
	if (vEndNode < 0 || vEndNode >= m_NodeCount) return false;

	m_AdjacentMatrix.coeffRef(vStartNode, vEndNode) = 1;
	m_AdjacentMatrix.coeffRef(vEndNode, vStartNode) = 1;
	m_AdjacentMatrix.makeCompressed();

	m_DenseAdjacentMatrix(vStartNode, vEndNode) = 1;
	m_DenseAdjacentMatrix(vEndNode, vStartNode) = 1;

	return true;
}

//****************************************************************************************************
//FUNCTION:
int CUndirectedGraph::findShortestDistanceByDijskra(int vSourceNodeIndex, int vDestinationNodeIndex) const
{
	_ASSERTE(vSourceNodeIndex >= 0 && vSourceNodeIndex < m_NodeCount);
	_ASSERTE(vDestinationNodeIndex >= 0 && vDestinationNodeIndex < m_NodeCount);

	std::vector<int> ShortestDistance = findShortestDistanceByDijskra(vSourceNodeIndex, std::vector<int>(1, vDestinationNodeIndex));
	_ASSERTE(ShortestDistance.size() == 1);

	return ShortestDistance[0];
}

//****************************************************************************************************
//FUNCTION:
std::vector<int> CUndirectedGraph::findShortestDistanceByDijskra(int vSourceNodeIndex, std::vector<int> vDestinationNodeIndices) const
{
	_ASSERTE(vSourceNodeIndex >= 0 && vSourceNodeIndex < m_NodeCount);

	const int DestinationNodeNum = vDestinationNodeIndices.size();
	std::vector<int> ShortestDistance(DestinationNodeNum, m_MaxDistance);
	boost::dynamic_bitset<> DestinationSet(m_NodeCount, false);
	for (auto Index : vDestinationNodeIndices)
	{
		_ASSERTE(Index >= 0 && Index < m_NodeCount);
		DestinationSet[Index] = true;
	}
	
	int FoundCount = 0;
	boost::dynamic_bitset<> VisitedSet(m_NodeCount, false);
	std::priority_queue<Node> PriorytyQueue;
	PriorytyQueue.push(Node(vSourceNodeIndex, 0));
	VisitedSet[vSourceNodeIndex] = true;
	while (!PriorytyQueue.empty())
	{
		Node TempNode = PriorytyQueue.top();
		PriorytyQueue.pop();

		if (DestinationSet[TempNode.Index])
		{
			auto Pos = std::find(vDestinationNodeIndices.begin(), vDestinationNodeIndices.end(), TempNode.Index);
			int Index = std::distance(vDestinationNodeIndices.begin(), Pos);
			ShortestDistance[Index] = TempNode.Distance;
			FoundCount++;
			if (FoundCount >= DestinationNodeNum) break;
		}

		for (int k = 0; k < m_NodeCount; k++)
		{
			if (!VisitedSet[k] && m_AdjacentMatrix.coeff(TempNode.Index, k) > 0)
			{
				VisitedSet[k] = true;
				PriorytyQueue.push(Node(k, TempNode.Distance + m_AdjacentMatrix.coeff(TempNode.Index, k)));
			}
		}
	}

	return ShortestDistance;
}

//****************************************************************************************************
//FUNCTION:
void CUndirectedGraph::generateShortestDistanceOf(int vNodeIndex)
{
	_ASSERTE(vNodeIndex >= 0 && vNodeIndex < m_NodeCount);

	std::vector<int> DestinationIndex(m_NodeCount, 0);
	for (int i = 0; i < m_NodeCount; i++) DestinationIndex[i] = i;
	
	std::vector<int> ShortestDistance = findShortestDistanceByDijskra(vNodeIndex, DestinationIndex);
#pragma omp critical
	{
		for (int i = 0; i < m_NodeCount; i++)
		{
			m_DenseAdjacentMatrix(vNodeIndex, DestinationIndex[i]) = ShortestDistance[i];
			m_DenseAdjacentMatrix(DestinationIndex[i], vNodeIndex) = ShortestDistance[i];
		}
	}
}

//****************************************************************************************************
//FUNCTION:
int CUndirectedGraph::findShortestDistance(int vSourceNodeIndex, int vDestinationNodeIndex) const
{
	_ASSERTE(vSourceNodeIndex >= 0 && vSourceNodeIndex < m_NodeCount);
	_ASSERTE(vDestinationNodeIndex >= 0 && vDestinationNodeIndex < m_NodeCount);

	int Distance = m_DenseAdjacentMatrix(vSourceNodeIndex, vDestinationNodeIndex);
	if (Distance >= m_MaxDistance)
		throw std::runtime_error("The shortest distance from " + std::to_string(vSourceNodeIndex) + " to " + std::to_string(vDestinationNodeIndex) + " haven't been generated.");

	return Distance;
}

//****************************************************************************************************
//FUNCTION:
std::vector<int> CUndirectedGraph::findShortestDistance(int vSourceNodeIndex, std::vector<int> vDestinationNodeIndices) const
{
	int NumOfDestinationNode = vDestinationNodeIndices.size();
	_ASSERTE(NumOfDestinationNode > 0);

	std::vector<int> ShortestDistance(NumOfDestinationNode, m_MaxDistance);
	for (int i = 0; i < NumOfDestinationNode; i++)
	{
		ShortestDistance[i] = findShortestDistance(vSourceNodeIndex, vDestinationNodeIndices[i]);
	}

	return ShortestDistance;
}